#include "cloud_transform.hpp"
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <fstream>
#include <sstream>
#include <iostream>

CloudTransform::CloudTransform()
    : old_cloud_(new pcl::PointCloud<pcl::PointXYZ>)
    , new_cloud_(new pcl::PointCloud<pcl::PointXYZ>)
    , transformed_cloud_(new pcl::PointCloud<pcl::PointXYZ>) {
}

bool CloudTransform::loadPointClouds(const std::string& old_cloud_path, 
                                   const std::string& new_cloud_path) {
    if (pcl::io::loadPLYFile(old_cloud_path, *old_cloud_) < 0) {
        std::cerr << "Error loading old cloud file: " << old_cloud_path << std::endl;
        return false;
    }
    
    if (pcl::io::loadPLYFile(new_cloud_path, *new_cloud_) < 0) {
        std::cerr << "Error loading new cloud file: " << new_cloud_path << std::endl;
        return false;
    }
    
    return true;
}

bool CloudTransform::loadPoseInfo(const std::string& pose_file_path) {
    std::ifstream file(pose_file_path);
    if (!file.is_open()) {
        std::cerr << "Error opening pose file: " << pose_file_path << std::endl;
        return false;
    }
    
    std::string line;
    // Skip header line
    std::getline(file, line);
    
    while (std::getline(file, line)) {
        std::stringstream ss(line);
        PoseInfo pose;
        
        // Read timestamp, RTK position, SLAM position, and euler angles
        ss >> pose.timestamp
           >> pose.rtk_position.x() >> pose.rtk_position.y() >> pose.rtk_position.z()
           >> pose.slam_position.x() >> pose.slam_position.y() >> pose.slam_position.z()
           >> pose.slam_euler.x() >> pose.slam_euler.y() >> pose.slam_euler.z();
        
        // Read covariance matrix (6x6)
        for (int i = 0; i < 6; ++i) {
            for (int j = 0; j < 6; ++j) {
                ss >> pose.covariance(i, j);
            }
        }
        
        pose_info_.push_back(pose);
    }
    
    return !pose_info_.empty();
}

Eigen::Matrix4f CloudTransform::getTransformationMatrix(const PoseInfo& pose) {
    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    
    // Convert euler angles to rotation matrix
    Eigen::AngleAxisf rollAngle(pose.slam_euler.x(), Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf pitchAngle(pose.slam_euler.y(), Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf yawAngle(pose.slam_euler.z(), Eigen::Vector3f::UnitZ());
    
    Eigen::Matrix3f rotation = (yawAngle * pitchAngle * rollAngle).matrix();
    
    // Set rotation part
    transform.block<3,3>(0,0) = rotation;
    
    // Set translation part
    transform(0,3) = pose.slam_position.x();
    transform(1,3) = pose.slam_position.y();
    transform(2,3) = pose.slam_position.z();
    
    return transform;
}

void CloudTransform::transformNewCloud() {
    if (pose_info_.empty()) {
        std::cerr << "No pose information available" << std::endl;
        return;
    }
    
    // Use the latest pose information to transform the cloud
    Eigen::Matrix4f transform = getTransformationMatrix(pose_info_.back());
    
    // Transform the new cloud
    pcl::transformPointCloud(*new_cloud_, *transformed_cloud_, transform);
}

double CloudTransform::evaluateCloudDifference() {
    if (!old_cloud_ || !transformed_cloud_) {
        std::cerr << "Clouds not initialized" << std::endl;
        return -1.0;
    }
    
    if (old_cloud_->size() != transformed_cloud_->size()) {
        std::cerr << "Cloud sizes don't match" << std::endl;
        return -1.0;
    }
    
    double total_distance = 0.0;
    int point_count = 0;
    
    // Calculate point-to-point distances
    for (size_t i = 0; i < old_cloud_->size(); ++i) {
        const auto& p1 = old_cloud_->points[i];
        const auto& p2 = transformed_cloud_->points[i];
        
        double distance = std::sqrt(
            std::pow(p1.x - p2.x, 2) +
            std::pow(p1.y - p2.y, 2) +
            std::pow(p1.z - p2.z, 2)
        );
        
        total_distance += distance;
        point_count++;
    }
    
    return total_distance / point_count; // Return average distance
}
